//
// Created by Ge on 2024/3/6.
//
#include "include/Matrix.h"
#include "math.h"

float length(float x, float y, float z) {
    return (float) sqrt(x * x + y * y + z * z);
}

void translateM(float *m, int mOffset,
                float x, float y, float z) {
    for (int i = 0; i < 4; i++) {
        int mi = mOffset + i;
        m[12 + mi] += m[mi] * x + m[4 + mi] * y + m[8 + mi] * z;
    }
}

bool overlap(
        float *a, int aStart, int aLength, float *b, int bStart, int bLength) {
    if (a != b) {
        return false;
    }
    if (aStart == bStart) {
        return true;
    }
    int aEnd = aStart + aLength;
    int bEnd = bStart + bLength;
    if (aEnd == bEnd) {
        return true;
    }
    if (aStart < bStart && bStart < aEnd) {
        return true;
    }
    if (aStart < bEnd && bEnd < aEnd) {
        return true;
    }
    if (bStart < aStart && aStart < bEnd) {
        return true;
    }
    if (bStart < aEnd && aEnd < bEnd) {
        return true;
    }
    return false;
}

void frustumM(float *m, int offset, float left, float right, float bottom, float top, float near,
              float far) {
    float r_width = 1.0f / (right - left);
    float r_height = 1.0f / (top - bottom);
    float r_depth = 1.0f / (near - far);
    float x = 2.0f * (near * r_width);
    float y = 2.0f * (near * r_height);
    float A = (right + left) * r_width;
    float B = (top + bottom) * r_height;
    float C = (far + near) * r_depth;
    float D = 2.0f * (far * near * r_depth);
    m[offset + 0] = x;
    m[offset + 5] = y;
    m[offset + 8] = A;
    m[offset + 9] = B;
    m[offset + 10] = C;
    m[offset + 14] = D;
    m[offset + 11] = -1.0f;
    m[offset + 1] = 0.0f;
    m[offset + 2] = 0.0f;
    m[offset + 3] = 0.0f;
    m[offset + 4] = 0.0f;
    m[offset + 6] = 0.0f;
    m[offset + 7] = 0.0f;
    m[offset + 12] = 0.0f;
    m[offset + 13] = 0.0f;
    m[offset + 15] = 0.0f;
}

void setLookAtM(float *rm, int rmOffset,
                float eyeX, float eyeY, float eyeZ,
                float centerX, float centerY, float centerZ, float upX, float upY,
                float upZ) {
    float fx = centerX - eyeX;
    float fy = centerY - eyeY;
    float fz = centerZ - eyeZ;

    // Normalize f
    float rlf = 1.0f / length(fx, fy, fz);
    fx *= rlf;
    fy *= rlf;
    fz *= rlf;

    // compute s = f x up (x means "cross product")
    float sx = fy * upZ - fz * upY;
    float sy = fz * upX - fx * upZ;
    float sz = fx * upY - fy * upX;

    // and normalize s
    float rls = 1.0f / length(sx, sy, sz);
    sx *= rls;
    sy *= rls;
    sz *= rls;

    // compute u = s x f
    float ux = sy * fz - sz * fy;
    float uy = sz * fx - sx * fz;
    float uz = sx * fy - sy * fx;

    rm[rmOffset + 0] = sx;
    rm[rmOffset + 1] = ux;
    rm[rmOffset + 2] = -fx;
    rm[rmOffset + 3] = 0.0f;

    rm[rmOffset + 4] = sy;
    rm[rmOffset + 5] = uy;
    rm[rmOffset + 6] = -fy;
    rm[rmOffset + 7] = 0.0f;

    rm[rmOffset + 8] = sz;
    rm[rmOffset + 9] = uz;
    rm[rmOffset + 10] = -fz;
    rm[rmOffset + 11] = 0.0f;

    rm[rmOffset + 12] = 0.0f;
    rm[rmOffset + 13] = 0.0f;
    rm[rmOffset + 14] = 0.0f;
    rm[rmOffset + 15] = 1.0f;

    translateM(rm, rmOffset, -eyeX, -eyeY, -eyeZ);
}

void multiplyMM(float *result, int resultOffset,
                float *lhs, int lhsOffset, float *rhs, int rhsOffset) {
    if (overlap(result, resultOffset, 16, lhs, lhsOffset, 16)
        || overlap(result, resultOffset, 16, rhs, rhsOffset, 16)) {
        float tmp[32];
        for (int i = 0; i < 4; i++) {
            float rhs_i0 = rhs[4 * i + 0 + rhsOffset];
            float ri0 = lhs[0 + lhsOffset] * rhs_i0;
            float ri1 = lhs[1 + lhsOffset] * rhs_i0;
            float ri2 = lhs[2 + lhsOffset] * rhs_i0;
            float ri3 = lhs[3 + lhsOffset] * rhs_i0;
            for (int j = 1; j < 4; j++) {
                float rhs_ij = rhs[4 * i + j + rhsOffset];
                ri0 += lhs[4 * j + 0 + lhsOffset] * rhs_ij;
                ri1 += lhs[4 * j + 1 + lhsOffset] * rhs_ij;
                ri2 += lhs[4 * j + 2 + lhsOffset] * rhs_ij;
                ri3 += lhs[4 * j + 3 + lhsOffset] * rhs_ij;
            }
            tmp[4 * i + 0] = ri0;
            tmp[4 * i + 1] = ri1;
            tmp[4 * i + 2] = ri2;
            tmp[4 * i + 3] = ri3;
        }

        // copy from tmp to result
        for (int i = 0; i < 16; i++) {
            result[i + resultOffset] = tmp[i];
        }

    } else {
        for (int i = 0; i < 4; i++) {
            float rhs_i0 = rhs[4 * i + 0 + rhsOffset];
            float ri0 = lhs[0 + lhsOffset] * rhs_i0;
            float ri1 = lhs[1 + lhsOffset] * rhs_i0;
            float ri2 = lhs[2 + lhsOffset] * rhs_i0;
            float ri3 = lhs[3 + lhsOffset] * rhs_i0;
            for (int j = 1; j < 4; j++) {
                float rhs_ij = rhs[4 * i + j + rhsOffset];
                ri0 += lhs[4 * j + 0 + lhsOffset] * rhs_ij;
                ri1 += lhs[4 * j + 1 + lhsOffset] * rhs_ij;
                ri2 += lhs[4 * j + 2 + lhsOffset] * rhs_ij;
                ri3 += lhs[4 * j + 3 + lhsOffset] * rhs_ij;
            }
            result[4 * i + 0 + resultOffset] = ri0;
            result[4 * i + 1 + resultOffset] = ri1;
            result[4 * i + 2 + resultOffset] = ri2;
            result[4 * i + 3 + resultOffset] = ri3;
        }
    }
}